function [U,Uxy,Z1_Pos_errors,distances] = quadrotor_controller3(x_new,Uxy,Uxy2,h0_Pos,leader,Des_Pos,Des_PSI,flag,U_Use)
%% Parameters
global m  g  dt I kt kd
global a1  a2  a3
global b1  b2  b3
global c1 c2 c3 c4 c5 c6
global c7 c8 c9 c10 c11 c12

%% columns of x input
columns = size(x_new,2);
format long;

%% Obtain adjacency matrix & reassign values to x
x = zeros(12,columns);
adjacency_Matrix = zeros(1,columns);
for Num = 1:columns
    adjacency_Matrix(1,Num) = x_new(1,Num);
    x(:,Num) = x_new(2:end,Num);
end

%% Initialization
x_Pos = zeros(3,columns);
x_Vel = zeros(3,columns);
X_Att = zeros(3,columns);
X_Att_Angle = zeros(3,columns);
for Num = 1:columns
    x_Pos(:,Num) = [x(7,Num); x(9,Num); x(11,Num)];       % X Y Z
    x_Vel(:,Num) = [x(8,Num); x(10,Num); x(12,Num)];      % X_dot Y_dot Z_dot
    X_Att(:,Num) = [x(1,Num); x(3,Num); x(5,Num)];        % PHI THETA PSI
    X_Att_Angle(:,Num) = [x(2,Num); x(4,Num); x(6,Num)];  % PHI_dot THETA_dot PSI_dot
end

%% Position Errors
% 1.Z1_Pos_errors
hY = zeros(3,columns); 
for Num = 1:columns
    temp3 = x_Pos(:,Num) - h0_Pos(:,Num); 
    hY(:,Num) = temp3;
end

% % Adjusting adjacency matrix
distances = zeros(columns,1);
% for Num = 1:columns
%     distances(Num) = norm(hY(:,Num) - hY(:,flag));
% end
% temp_adjMat = zeros(1,columns);
% rin = 4.5; rout = 30; % 通过距离获得rin rout
% for Num = 1:columns
%     distances(Num) = norm(hY(:,Num) - hY(:,flag));
%     if (distances(Num) < rin)
%         temp_adjMat(Num) = 1;
%     elseif (distances(Num) > rout)
%         temp_adjMat(Num) = 0;
%     else
%         temp_adjMat(Num) = 1 - 0.5*(cos((rout - distances(Num))/(rout-rin)*pi) + 1);
%     end   
% end
% adjacency_Matrix = adjacency_Matrix .* temp_adjMat;

temp1 = leader*(hY(:,flag) - Des_Pos(:,1)); 
temp2 = zeros(3,1);
for Num = 1:columns
    temp2 = adjacency_Matrix(Num)*(hY(:,flag) - hY(:,Num)) + temp2;
end
Z1_Pos_errors = temp2 + temp1;

% aij+bi
temp3 = 0;
for Num = 1:columns
    temp3 = adjacency_Matrix(Num) + temp3;
end
temp4 = temp3 + leader';

% 2.alpha
temp5 = zeros(3,1);
temp5(1) = leader*Des_Pos(1,2) - c7*Z1_Pos_errors(1);  % Ux
temp5(2) = leader*Des_Pos(2,2) - c9*Z1_Pos_errors(2);  % Uy
temp5(3) = leader*Des_Pos(3,2) - c11*Z1_Pos_errors(3); % Uz

temp6 = zeros(3,1);
for Num = 1:columns
    temp6 = adjacency_Matrix(Num)*x_Vel(:,Num) + temp6;
end
temp7 = temp5 + temp6;
alpha = temp7/(temp4);

% 3.Z1_Pos_errors_dot
temp8 = leader*(x_Vel(:,flag) - Des_Pos(:,2));
temp9 = zeros(3,1);
for Num = 1:columns
    temp9 = adjacency_Matrix(Num)*(x_Vel(:,flag) - x_Vel(:,Num)) + temp9;
end
temp10 = temp8 + temp9;
Z1_Pos_errors_dot = temp10;

% 4.alpha_dot
temp11 = zeros(3,1);
temp11(1) = leader*Des_Pos(1,3) - c7*Z1_Pos_errors_dot(1); % Ux
temp11(2) = leader*Des_Pos(2,3) - c9*Z1_Pos_errors_dot(2); % Uy
temp11(3) = leader*Des_Pos(3,3) - c11*Z1_Pos_errors_dot(3); % Uz
temp12 = zeros(3,1);
Ux1 = zeros(columns,1); Uy1 = zeros(columns,1); V_dot = zeros(3,columns);
for Num = 1:columns
    Ux1(Num) = cos(X_Att(1,Num))*sin(X_Att(2,Num))*cos(X_Att(3,Num)) + sin(X_Att(1,Num))*sin(X_Att(3,Num));
    Uy1(Num) = cos(X_Att(1,Num))*sin(X_Att(2,Num))*sin(X_Att(3,Num)) - sin(X_Att(1,Num))*cos(X_Att(3,Num));
    V_dot(1,Num) = Ux1(Num)*U_Use(1,Num)/m;
    V_dot(2,Num) = Uy1(Num)*U_Use(1,Num)/m;
    V_dot(3,Num) = -g + cos(X_Att(1,Num))*cos(X_Att(2,Num))*U_Use(1,Num)/m;
end
temp14 = zeros(3,1);
for Num = 1:columns
    temp14 = adjacency_Matrix(Num)*V_dot(:,Num) + temp14;
end
temp13 = temp11 + temp12;
alpha_dot = temp13/(temp4);

% 5.Z2_Pos_errors
Z2_Pos_errors = x_Vel(:,flag) - alpha;

%% Controllers X Y Z
% Controller of Z
z7 = Z1_Pos_errors(3);
z8 = Z2_Pos_errors(3);
fx_z = -g; gx_z = cos(x(1,flag))*cos(x(3,flag))/m;
U1 = (-fx_z - z7 + alpha_dot(3) - c12*z8)/gx_z;

% Controller of X
z9 = Z1_Pos_errors(1);
z10 = Z2_Pos_errors(1);
fx_x = 0; gx_x = U1/m;
Ux = (-fx_x - z9 + alpha_dot(1) - c8*z10)/gx_x;

% Controller of Y
z11 = Z1_Pos_errors(2);
z12 = Z2_Pos_errors(2);
fx_y = 0; gx_y = U1/m;
Uy = (-fx_y - z11 + alpha_dot(2) - c10*z12)/gx_y;

% Ux_dot、Uy_dot
Ux_dot = (Ux - Uxy(1))/dt;        % Ux近似导数
Ux_2dot = (Ux - Uxy2(1))/2/dt;    % Ux近似二阶导数
Uy_dot = (Uy - Uxy(2))/dt;        % Ux近似导数
Uy_2dot = (Uy - Uxy2(2))/2/dt;    % Ux近似二阶导数

%% Desire Att
% THETAd
THETAd = Ux;  
THETAd_dot = Ux_dot;
THETAd_2dot = Ux_2dot;

% PHId
PHId = -Uy;  
PHId_dot = -Uy_dot;
PHId_2dot = -Uy_2dot;

Des_Att = [PHId       PHId_dot   PHId_2dot;
           THETAd     THETAd_dot THETAd_2dot;
           Des_PSI(1) Des_PSI(2) Des_PSI(3)];
%% Attitude errors
% Z1_Att_errors
Z1_Att_errors = X_Att(:,flag) - Des_Att(:,1); 

% alpha_Att
alpha_Att = zeros(3,1);
alpha_Att(1) =  Des_Att(1,2) - c1*Z1_Att_errors(1);
alpha_Att(2) =  Des_Att(2,2) - c3*Z1_Att_errors(2);
alpha_Att(3) =  Des_Att(3,2) - c5*Z1_Att_errors(3);

% Z1_Att_errors_dot
Z1_Att_errors_dot = X_Att_Angle(:,flag) - Des_Att(:,2);

% alpha_Att_dot
alpha_Att_dot = zeros(3,1);
alpha_Att_dot(1) = Des_Att(1,3) - c1*Z1_Att_errors_dot(1);
alpha_Att_dot(2) = Des_Att(2,3) - c1*Z1_Att_errors_dot(2);
alpha_Att_dot(3) = Des_Att(3,3) - c1*Z1_Att_errors_dot(3);

% Z2_Att_errors
Z2_Att_errors = X_Att_Angle(:,flag) - alpha_Att;

%% Controller U2 U3 U4
omega1 = sqrt(abs(U_Use(1,flag)/(4*kt) + U_Use(3,flag)/(2*kt) + U_Use(4,flag)/(4*kd)));
omega2 = sqrt(abs(U_Use(1,flag)/(4*kt) - U_Use(2,flag)/(2*kt) - U_Use(4,flag)/(4*kd)));
omega3 = sqrt(abs(U_Use(1,flag)/(4*kt) - U_Use(3,flag)/(2*kt) + U_Use(4,flag)/(4*kd)));
omega4 = sqrt(abs(U_Use(1,flag)/(4*kt) + U_Use(2,flag)/(2*kt) - U_Use(4,flag)/(4*kd)));
omega = omega1 - omega2 + omega3 - omega4;

fx_phi = a1*x(4,flag)*x(6,flag) - I*x(4,flag)*omega; gx_phi = b1;
fx_theta = a2*x(2,flag)*x(6,flag) + I*x(2,flag)*omega; gx_theta = b2;
fx_psi = a3*x(2,flag)*x(4,flag); gx_psi = b3;

U_Att = zeros(3,1);
U_Att(1) = (- Z1_Att_errors(1) - fx_phi   + alpha_Att_dot(1) - c2*Z2_Att_errors(1))/gx_phi;
U_Att(2) = (- Z1_Att_errors(2) - fx_theta + alpha_Att_dot(2) - c4*Z2_Att_errors(2) )/gx_theta;
U_Att(3) = (- Z1_Att_errors(3) - fx_psi   + alpha_Att_dot(3) - c6*Z2_Att_errors(3))/gx_psi;

% U_Att(1) = (- Z1_Att_errors(1) - fx_phi   - c2*Z2_Att_errors(1))/gx_phi;
% U_Att(2) = (- Z1_Att_errors(2) - fx_theta  - c4*Z2_Att_errors(2) )/gx_theta;
% U_Att(3) = (- Z1_Att_errors(3) - fx_psi  - c6*Z2_Att_errors(3))/gx_psi;

%% U
U2 = U_Att(1); U3 = U_Att(2); U4 = U_Att(3);
U = [U1; U2; U3; U4];
Uxy = [Ux; Uy];
end